/*
 * COPYRIGHT (C) 2005-2011
 * RE2, INC.
 * ALL RIGHTS RESERVED
 *
 *
 * THIS WORK CONTAINS VALUABLE CONFIDENTIAL AND PROPRIETARY INFORMATION.
 * DISCLOSURE OR REPRODUCTION WITHOUT THE WRITTEN AUTHORIZATION OF RE2, INC.
 * IS PROHIBITED. THIS UNPUBLISHED WORK BY RE2, INC. IS PROTECTED BY THE LAWS
 * OF THE UNITED STATES AND OTHER COUNTRIES. IF PUBLICATION OF THE WORK SHOULD
 * OCCUR, THE FOLLOWING NOTICE SHALL APPLY.
 *
 * "COPYRIGHT (C) 2005-2011 RE2, INC. ALL RIGHTS RESERVED."
 *
 * RE2, INC. DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE, INCLUDING
 * ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS, IN NO EVENT SHALL
 * RE2, INC. BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR
 * ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER
 * IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT
 * OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */

#include "StatePublisher.h"
#include <robot_state_publisher/robot_state_publisher.h>
#include <kdl_parser/kdl_parser.hpp>
#include <urdf/model.h>
#include <ros/ros.h>
#include <WAM_msgs/ArmPositions.h>
#include <WAM_msgs/ArmProperties.h>
#include <WAM_msgs/ArmPositionProperties.h>
#include <WAM_msgs/ArmTorques.h>
#include <WAM_msgs/ArmVelocities.h>
#include <WAM_msgs/WAMEnums.h>

#include <DPPanTilt_msgs/ReportDynamicInfo.h>

StatePublisher::StatePublisher(const ros::NodeHandle& nodeHandle, const std::string& urdfData) :
    m_nodeHandle(nodeHandle), m_jointMap(NULL), m_isRunning(false)
{
    urdf::Model model;
    KDL::Tree tree;
    char *numbers[10] = {"Zero", "One", "Two", "Three", "Four"}; // Add em if you got em

    m_jointMap = new std::map<std::string, double>();

    /// Left WAM
    m_leftWamJointNames[WAM_YAW_ID] = "DarmSim/LeftArm/WAM/YawJoint";
    m_leftWamJointNames[WAM_SHOULDERPITCH_ID] = "DarmSim/LeftArm/WAM/ShoulderPitchJoint";
    m_leftWamJointNames[WAM_SHOULDERYAW_ID] = "DarmSim/LeftArm/WAM/ShoulderYawJoint";
    m_leftWamJointNames[WAM_ELBOW_ID] = "DarmSim/LeftArm/WAM/ElbowJoint";
    m_leftWamJointNames[WAM_UPPERWRISTYAW_ID] = "DarmSim/LeftArm/WAM/UpperWristYawJoint";
    m_leftWamJointNames[WAM_WRISTPITCH_ID] = "DarmSim/LeftArm/WAM/UpperWristPitchJoint";
    m_leftWamJointNames[WAM_LOWERWRISTYAW_ID] = "DarmSim/LeftArm/WAM/LowerWristYawJoint";
    
    /// Right WAM
    m_rightWamJointNames[WAM_YAW_ID] = "DarmSim/RightArm/WAM/YawJoint";
    m_rightWamJointNames[WAM_SHOULDERPITCH_ID] = "DarmSim/RightArm/WAM/ShoulderPitchJoint";
    m_rightWamJointNames[WAM_SHOULDERYAW_ID] = "DarmSim/RightArm/WAM/ShoulderYawJoint";
    m_rightWamJointNames[WAM_ELBOW_ID] = "DarmSim/RightArm/WAM/ElbowJoint";
    m_rightWamJointNames[WAM_UPPERWRISTYAW_ID] = "DarmSim/RightArm/WAM/UpperWristYawJoint";
    m_rightWamJointNames[WAM_WRISTPITCH_ID] = "DarmSim/RightArm/WAM/UpperWristPitchJoint";
    m_rightWamJointNames[WAM_LOWERWRISTYAW_ID] = "DarmSim/RightArm/WAM/LowerWristYawJoint";
    
    // Left Hand
    m_leftBHandJointNames[BHAND_FINGERONE][0] = "DarmSim/LeftArm/BHand/FingerOne/KnuckleOneJoint";
    m_leftBHandJointNames[BHAND_FINGERONE][1] = "DarmSim/LeftArm/BHand/FingerOne/KnuckleTwoJoint";
    m_leftBHandJointNames[BHAND_FINGERONE][2] = "DarmSim/LeftArm/BHand/FingerOne/KnuckleThreeJoint";
    m_leftBHandJointNames[BHAND_FINGERTWO][0] = "DarmSim/LeftArm/BHand/FingerTwo/KnuckleOneJoint";
    m_leftBHandJointNames[BHAND_FINGERTWO][1] = "DarmSim/LeftArm/BHand/FingerTwo/KnuckleTwoJoint";
    m_leftBHandJointNames[BHAND_FINGERTWO][2] = "DarmSim/LeftArm/BHand/FingerTwo/KnuckleThreeJoint";
    m_leftBHandJointNames[BHAND_FINGERTHREE][0] = "DarmSim/LeftArm/BHand/FingerThree/KnuckleOneJoint";
    m_leftBHandJointNames[BHAND_FINGERTHREE][1] = "DarmSim/LeftArm/BHand/FingerThree/KnuckleTwoJoint";
    m_leftBHandJointNames[BHAND_FINGERTHREE][2] = "DarmSim/LeftArm/BHand/FingerThree/KnuckleThreeJoint";

    // Right Hand
    m_rightBHandJointNames[BHAND_FINGERONE][0] = "DarmSim/RightArm/BHand/FingerOne/KnuckleOneJoint";
    m_rightBHandJointNames[BHAND_FINGERONE][1] = "DarmSim/RightArm/BHand/FingerOne/KnuckleTwoJoint";
    m_rightBHandJointNames[BHAND_FINGERONE][2] = "DarmSim/RightArm/BHand/FingerOne/KnuckleThreeJoint";
    m_rightBHandJointNames[BHAND_FINGERTWO][0] = "DarmSim/RightArm/BHand/FingerTwo/KnuckleOneJoint";
    m_rightBHandJointNames[BHAND_FINGERTWO][1] = "DarmSim/RightArm/BHand/FingerTwo/KnuckleTwoJoint";
    m_rightBHandJointNames[BHAND_FINGERTWO][2] = "DarmSim/RightArm/BHand/FingerTwo/KnuckleThreeJoint";
    m_rightBHandJointNames[BHAND_FINGERTHREE][0] = "DarmSim/RightArm/BHand/FingerThree/KnuckleOneJoint";
    m_rightBHandJointNames[BHAND_FINGERTHREE][1] = "DarmSim/RightArm/BHand/FingerThree/KnuckleTwoJoint";
    m_rightBHandJointNames[BHAND_FINGERTHREE][2] = "DarmSim/RightArm/BHand/FingerThree/KnuckleThreeJoint";


    (*m_jointMap)["DarmSim/LowerPanTilt/Pan"] = 0;
    (*m_jointMap)["DarmSim/LowerPanTilt/Tilt"] = 0;
    (*m_jointMap)["DarmSim/lowerTiltFrontJoint"] = 0;
    (*m_jointMap)["DarmSim/lowerTiltTopJoint"] = 0;
    (*m_jointMap)["DarmSim/UpperPanTilt/Pan"] = 0;
    (*m_jointMap)["DarmSim/UpperPanTilt/Tilt"] = 0;
    (*m_jointMap)["BumbleBee2LeftEyeJoint"] = 0;
    (*m_jointMap)["SR4000Joint"] = 0;
    (*m_jointMap)["RightEyeJoint"] = 0;
    (*m_jointMap)["ProsilicaJoint"] = 0;

    for (int i = 0; i < WAM_DOF; i++)
    {
        (*m_jointMap)[m_leftWamJointNames[i]] = 0.0;
        (*m_jointMap)[m_rightWamJointNames[i]] = 0.0;
    }

    for (int i=0; i<3; ++i) {
    	for (int j=0; j<3; ++j) {
			(*m_jointMap)[m_leftBHandJointNames[i][j]] = 0.0;
			(*m_jointMap)[m_rightBHandJointNames[i][j]] = 0.0;
    	}
    }


    if (!model.initString(urdfData)) {
        ROS_ERROR("Failed to parse urdf file %s", urdfData.c_str());
    }
    else {
        ROS_INFO("Parsed URDF file");
    }

    if (!kdl_parser::treeFromUrdfModel(model, tree)) {
        ROS_ERROR("Failed to construct kdl tree");
    }

    m_publisher = new robot_state_publisher::RobotStatePublisher(tree);

}

StatePublisher::~StatePublisher()
{
    delete m_jointMap;

    m_isRunning = false;
}

void StatePublisher::start()
{

    m_leftWamPositionsSubscriber
            = m_nodeHandle.subscribe("/LeftArm/WAM/arm_positions", 1, &StatePublisher::leftWamPositionsCallback, this);
    m_rightWamPositionsSubscriber
            = m_nodeHandle.subscribe("/RightArm/WAM/arm_positions", 1, &StatePublisher::rightWamPositionsCallback, this);
    m_upperPTUSubscriber
            = m_nodeHandle.subscribe("/UpperPanTilt/report_dynamic_info", 1, &StatePublisher::upperPTUCallback, this);
    m_lowerPTUSubscriber
            = m_nodeHandle.subscribe("/LowerPanTilt/report_dynamic_info", 1, &StatePublisher::lowerPTUCallback, this);
    m_rightHandSub = m_nodeHandle.subscribe("/RightArm/BHand/bhand_positions",1,&StatePublisher::rightHandPositionsCallback,this);
    m_leftHandSub = m_nodeHandle.subscribe("/LeftArm/BHand/bhand_positions",1,&StatePublisher::leftHandPositionsCallback,this);

    m_isRunning = true;
    loop();
}

void StatePublisher::stop()
{
    m_leftWamPositionsSubscriber.shutdown();
    m_rightWamPositionsSubscriber.shutdown();
    m_lowerPTUSubscriber.shutdown();
    m_upperPTUSubscriber.shutdown();
    m_rightHandSub.shutdown();
    m_leftHandSub.shutdown();
}

void StatePublisher::loop()
{
    while (m_isRunning && ros::ok())
    {
        usleep(10000);
        if (!m_publisher->publishTransforms(*m_jointMap, ros::Time::now()))
        {
            ROS_WARN("Couldn't publish tf state.");
        }
        ros::spinOnce();
    }
}

void StatePublisher::rightHandPositionsCallback(const BHand_msgs::BHandPositionsConstPtr& msg)
{
	for (int i=0; i<3; ++i) {

			if (i == 2) (*m_jointMap)[m_rightBHandJointNames[i][0]] = 0.0;
			else (*m_jointMap)[m_rightBHandJointNames[i][0]] = msg->bhandBreakawayPositions[3];
			(*m_jointMap)[m_rightBHandJointNames[i][1]] = msg->bhandPositions[i];
			(*m_jointMap)[m_rightBHandJointNames[i][2]] = msg->bhandBreakawayPositions[i];
	}

	if (!m_publisher->publishTransforms(*m_jointMap, ros::Time::now())) {
		ROS_WARN("Couldn't publish tf state");
	}
}

void StatePublisher::leftHandPositionsCallback(const BHand_msgs::BHandPositionsConstPtr& msg)
{
	for (int i=0; i<3; ++i) {

			if (i == 1) (*m_jointMap)[m_leftBHandJointNames[i][0]] = 0.0;
			else (*m_jointMap)[m_leftBHandJointNames[i][0]] = msg->bhandPositions[3];
			(*m_jointMap)[m_leftBHandJointNames[i][1]] = msg->bhandPositions[i];
			(*m_jointMap)[m_leftBHandJointNames[i][2]] = msg->bhandBreakawayPositions[i];

	}

	if (!m_publisher->publishTransforms(*m_jointMap, ros::Time::now())) {
		ROS_WARN("Couldn't publish tf state");
	}
}

void StatePublisher::leftWamPositionsCallback(const WAM_msgs::ArmPositionsConstPtr& msg)
{

    for (int i = 0; i < WAM_DOF; i++)
    {
        (*m_jointMap)[m_leftWamJointNames[i]] = msg->wamPositions[i];
    }

    if (!m_publisher->publishTransforms(*m_jointMap, ros::Time::now()))
    {
        ROS_WARN("Couldn't publish tf state.");
    }
}

void StatePublisher::rightWamPositionsCallback(const WAM_msgs::ArmPositionsConstPtr& msg)
{

    for (int i = 0; i < WAM_DOF; i++)
    {
        (*m_jointMap)[m_rightWamJointNames[i]] = msg->wamPositions[i];
    }

    if (!m_publisher->publishTransforms(*m_jointMap, ros::Time::now()))
    {
        ROS_WARN("Couldn't publish tf state.");
    }
}

void StatePublisher::upperPTUCallback(const DPPanTilt_msgs::ReportDynamicInfoConstPtr& msg)
{
    (*m_jointMap)["DarmSim/UpperPanTilt/Pan"] = msg->panPositionInRadians;
    (*m_jointMap)["DarmSim/UpperPanTilt/Tilt"] = msg->tiltPositionInRadians;

    if (!m_publisher->publishTransforms(*m_jointMap, ros::Time::now()))
    {
        ROS_WARN("Couldn't publish tf state.");
    }
}

void StatePublisher::lowerPTUCallback(const DPPanTilt_msgs::ReportDynamicInfoConstPtr& msg)
{
    (*m_jointMap)["DarmSim/LowerPanTilt/Pan"] = msg->panPositionInRadians;
    (*m_jointMap)["DarmSim/LowerPanTilt/Tilt"] = msg->tiltPositionInRadians;

    if (!m_publisher->publishTransforms(*m_jointMap, ros::Time::now()))
    {
        ROS_WARN("Couldn't publish tf state.");
    }
}
